#include "track/cost.h"

#include <algorithm>

namespace autodrive {
namespace perception {
Cost::Cost() noexcept {}

/// @brief compute location distance
float Cost::ComputerLocationDistance(
    ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
    TrackedObjectPtr const& new_object) noexcept {
  // const TrackedObjectPtr& last_object = track->current_object_;
  // Eigen::Vector2f measured_anchor_point = new_object->anchor_point.head(2);
  // Eigen::Vector2f predicted_anchor_point = track_predict.head(2);
  // Eigen::Vector2f measurement_predict_diff =
  //     measured_anchor_point - predicted_anchor_point;
  // float location_distance = measurement_predict_diff.norm();

  // Eigen::Vector2f track_motion_dir = last_object->velocity.head(2);
  // float track_speed = track_motion_dir.norm();
  // track_motion_dir /= track_speed;

  // // Assume location distance is generated from a normal distribution with
  // // symmetric variance . Modify its variance when track speed is greater
  // than
  // // a threshold. Penalize variance in the orthogonal direction of motion.
  // if (track_speed > 2) {
  //   Eigen::Vector2f track_motion_orthogonal_dir =
  //       Eigen::Vector2f(track_motion_dir(1), -track_motion_dir(0));
  //   float motion_dir_distane =
  //       track_motion_dir(0) * measurement_predict_diff(0) +
  //       track_motion_dir(1) * measurement_predict_diff(1);
  //   float motion_orthogonal_dir_distance =
  //       track_motion_orthogonal_dir(0) * measurement_predict_diff(0) +
  //       track_motion_orthogonal_dir(1) * measurement_predict_diff(1);
  //   location_distance = sqrt(motion_dir_distane * motion_dir_distane * 0.25 +
  //                            motion_orthogonal_dir_distance *
  //                                motion_orthogonal_dir_distance * 4);
  // }
  // return location_distance;
  return 0.0f;
}

/// @brief compute direction distance
float Cost::ComputerDirectionDistance(
    ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
    TrackedObjectPtr const& new_object) noexcept {
  // const TrackedObjectPtr& last_object = track->current_object_;
  // Eigen::Vector3f old_anchor_point = last_object->anchor_point;
  // Eigen::Vector3f new_anchor_point = new_object->anchor_point;
  // Eigen::Vector3f anchor_point_shift = new_anchor_point - old_anchor_point;
  // anchor_point_shift(2) = 0;
  // Eigen::Vector3f predicted_track_motion = track_predict.head(6).tail(3);
  // predicted_track_motion(2) = 0;

  // double cos_theta = 0.994;  // average cos
  // if (!anchor_point_shift.head(2).isZero() &&
  //     !predicted_track_motion.head(2).isZero()) {
  //   cos_theta = LidarMathUtil::VectorCosTheta2fXy(predicted_track_motion,
  //                                                 anchor_point_shift);
  // }
  // float direction_distance = -cos_theta + 1.0;
  // return direction_distance;
  return 0.0f;
}

/// @brief compute bbox size distance
float Cost::ComputerBBoxSizeDistance(
    ObjectTrackPtr const& track, TrackedObjectPtr const& new_object) noexcept {
  // const TrackedObjectPtr& last_object = track->current_object_;
  // Eigen::Vector3f old_bbox_dir = last_object->direction;
  // Eigen::Vector3f new_bbox_dir = new_object->direction;
  // Eigen::Vector3f old_bbox_size = last_object->size;
  // Eigen::Vector3f new_bbox_size = new_object->direction;

  // float size_distance = 0.0;
  // double dot_val_00 = fabs(old_bbox_dir(0) * new_bbox_dir(0) +
  //                          old_bbox_dir(1) * new_bbox_dir(1));
  // double dot_val_01 = fabs(old_bbox_dir(0) * new_bbox_dir(1) -
  //                          old_bbox_dir(1) * new_bbox_dir(0));
  // bool bbox_dir_close = dot_val_00 > dot_val_01;

  // if (bbox_dir_close) {
  //   float diff_1 = fabs(old_bbox_size(0) - new_bbox_size(0)) /
  //                  std::max(old_bbox_size(0), new_bbox_size(0));
  //   float diff_2 = fabs(old_bbox_size(1) - new_bbox_size(1)) /
  //                  std::max(old_bbox_size(1), new_bbox_size(1));
  //   size_distance = std::min(diff_1, diff_2);

  // } else {
  //   float diff_1 = fabs(old_bbox_size(0) - new_bbox_size(1)) /
  //                  std::max(old_bbox_size(0), new_bbox_size(1));
  //   float diff_2 = fabs(old_bbox_size(1) - new_bbox_size(0)) /
  //                  std::max(old_bbox_size(1), new_bbox_size(0));
  //   size_distance = std::min(diff_1, diff_2);
  // }
  // return size_distance;
  return 0.0f;
}

float Cost::ComputerCartesianDistance2D(
    ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
    TrackedObjectPtr const& new_object) noexcept {
  Box b1;
  Box b2;
  object_to_box(track->current_object_, b1);
  object_to_box(new_object, b2);

  float x_diff = track_predict(3) * track_predict(6);
  float y_diff = track_predict(4) * track_predict(6);
  // update new object center using predict
  b1.center.x() = b1.center.x() + x_diff;
  b1.center.y() = b1.center.y() + y_diff;

  float dis = 0.0f;
  dis = sqrt((b1.center.x() - b2.center.x()) * (b1.center.x() - b2.center.x()) +
             (b1.center.y() - b2.center.y()) * (b1.center.y() - b2.center.y()));
  return dis;
}

/// @brief compute track&new object Iou2d
float Cost::ComputerIOU2D(ObjectTrackPtr const& track,
                          Eigen::VectorXf const& track_predict,
                          TrackedObjectPtr const& new_object) noexcept {
  Box b1;
  Box b2;
  object_to_box(track->current_object_, b1);
  object_to_box(new_object, b2);

  float x_diff = track_predict(3) * track_predict(6);
  float y_diff = track_predict(4) * track_predict(6);
  // update new object center using predict
  b1.center.x() = b1.center.x() + x_diff;
  b1.center.y() = b1.center.y() + y_diff;
  // update new object polygen using predict
  for (auto& polygen : b1.polygon) {
    polygen.x() = polygen.x() + x_diff;
    polygen.y() = polygen.y() + y_diff;
  }

  return iou2d(b1, b2);
}

/// @brief compute track&new object Iou2d
float Cost::ComputerIOU2DWithPos(ObjectTrackPtr const& track,
                                 Eigen::VectorXf const& track_predict,
                                 TrackedObjectPtr const& new_object,
                                 const Eigen::Matrix4f& pos) noexcept {
  Box b1;
  Box b2;
  object_to_box(track->current_object_, b1, pos);
  object_to_box(new_object, b2);

  float x_diff = track_predict(3) * track_predict(6);
  float y_diff = track_predict(4) * track_predict(6);
  // update new object center using predict
  b1.center.x() = b1.center.x() + x_diff;
  b1.center.y() = b1.center.y() + y_diff;
  // update new object polygen using predict
  for (auto& polygen : b1.polygon) {
    polygen.x() = polygen.x() + x_diff;
    polygen.y() = polygen.y() + y_diff;
  }

  return iou2d(b1, b2);
}

/// @brief compute track&new object Iou3d
float Cost::ComputerIOU3D(ObjectTrackPtr const& track,
                          Eigen::VectorXf const& track_predict,
                          TrackedObjectPtr const& new_object) noexcept {
  Box b1;
  Box b2;
  object_to_box(track->current_object_, b1);
  object_to_box(new_object, b2);

  float x_diff = track_predict(3) * track_predict(6);
  float y_diff = track_predict(4) * track_predict(6);
  float z_diff = track_predict(5) * track_predict(6);
  // update new object center using predict
  b1.center.x() = b1.center.x() + x_diff;
  b1.center.y() = b1.center.y() + y_diff;
  b1.center.z() = b1.center.z() + z_diff;
  // update new object polygen using predict
  for (auto& polygen : b1.polygon) {
    polygen.x() = polygen.x() + x_diff;
    polygen.y() = polygen.y() + y_diff;
  }

  return iou3d(b1, b2);
}

/// @brief compute track&new object gIou2d
float Cost::ComputerGIOU2D(ObjectTrackPtr const& track,
                           Eigen::VectorXf const& track_predict,
                           TrackedObjectPtr const& new_object) noexcept {
  Box b1;
  Box b2;
  object_to_box(track->current_object_, b1);
  object_to_box(new_object, b2);

  float x_diff = track_predict(3) * track_predict(6);
  float y_diff = track_predict(4) * track_predict(6);
  // update new object center using predict
  b1.center.x() = b1.center.x() + x_diff;
  b1.center.y() = b1.center.y() + y_diff;
  // update new object polygen using predict
  for (auto& polygen : b1.polygon) {
    polygen.x() = polygen.x() + x_diff;
    polygen.y() = polygen.y() + y_diff;
  }

  return giou2d(b1, b2);
}

/// @brief compute track&new object Iou3d
float Cost::ComputerGIOU3D(ObjectTrackPtr const& track,
                           Eigen::VectorXf const& track_predict,
                           TrackedObjectPtr const& new_object) noexcept {
  Box b1;
  Box b2;
  object_to_box(track->current_object_, b1);
  object_to_box(new_object, b2);

  float x_diff = track_predict(3) * track_predict(6);
  float y_diff = track_predict(4) * track_predict(6);
  float z_diff = track_predict(5) * track_predict(6);
  // update new object center using predict
  b1.center.x() = b1.center.x() + x_diff;
  b1.center.y() = b1.center.y() + y_diff;
  b1.center.z() = b1.center.z() + z_diff;
  // update new object polygen using predict
  for (auto& polygen : b1.polygon) {
    polygen.x() = polygen.x() + x_diff;
    polygen.y() = polygen.y() + y_diff;
    polygen.z() = polygen.z() + z_diff;
  }

  return giou3d(b1, b2);
}

float Cost::ComputerCartesian2DDistance(
    const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict,
    const TrackedObjectPtr& new_object) noexcept {
  Eigen::Vector2f measured_center_point = new_object->center.head(2);
  Eigen::Vector2f tracked_center_point = Eigen::Vector2f{
      track->current_object_->center.x() + track_predict(3) * track_predict(6),
      track->current_object_->center.y() + track_predict(4) * track_predict(6)};
  float distance = (measured_center_point - tracked_center_point).norm();

  return distance;
}

float Cost::ComputerCartesian2DDistanceWithPos(
    ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
    TrackedObjectPtr const& new_object, const Eigen::Matrix4f& pos) noexcept {
  Eigen::Vector4f temp_center =
      pos * Eigen::Vector4f{track->current_object_->center.x(),
                            track->current_object_->center.y(),
                            track->current_object_->center.z(), 1.0f};

  Eigen::Vector2f measured_center_point = new_object->center.head(2);
  Eigen::Vector2f tracked_center_point =
      Eigen::Vector2f{temp_center.x() + track_predict(3) * track_predict(6),
                      temp_center.y() + track_predict(4) * track_predict(6)};
  float distance = (measured_center_point - tracked_center_point).norm();

  return distance;
}

float Cost::iou2d(const Box& b1, const Box& b2) {
  polygon_t poly1;
  polygon_t poly2;

  for (auto& pp : b1.polygon) {
    point_t p1{pp.x(), pp.y()};
    bg::append(poly1, p1);
  }

  for (auto& pp : b2.polygon) {
    point_t p2{pp.x(), pp.y()};
    bg::append(poly2, p2);
  }

  polygon_t hull1;
  polygon_t hull2;
  bg::convex_hull(poly1, hull1);
  bg::convex_hull(poly2, hull2);

  std::deque<polygon_t> output;
  auto isOK = bg::intersection(hull1, hull2, output);
  if (!isOK) {
    // throw std::logic_error("polygen intersection error!");
  }

  double i{0};
  BOOST_FOREACH (polygon_t const& p, output) {
    i += bg::area(p);
  }  // 求交面积

  auto area1 = bg::area(hull1);
  auto area2 = bg::area(hull2);
  auto u{area1 + area2 - i};  // union面积
  if (u == 0) {
    // throw std::out_of_range("Invalid iou, u is 0");
  }

  return i / u;  // IOU
}

float Cost::iou3d(const Box& b1, const Box& b2) {
  polygon_t poly1;
  polygon_t poly2;

  for (auto pp : b1.polygon) {
    point_t p1{pp.x(), pp.y()};
    bg::append(poly1, p1);
  }

  for (auto pp : b2.polygon) {
    point_t p2{pp.x(), pp.y()};
    bg::append(poly2, p2);
  }

  polygon_t hull1;
  polygon_t hull2;
  bg::convex_hull(poly1, hull1);
  bg::convex_hull(poly2, hull2);

  std::deque<polygon_t> output;
  auto isOK = bg::intersection(hull1, hull2, output);
  if (!isOK) {
    throw std::logic_error("polygen intersection error!");
  }

  double i{0};
  BOOST_FOREACH (polygon_t const& p, output) {
    i += bg::area(p);
  }  // 求交面积

  auto area1 = bg::area(hull1);
  auto area2 = bg::area(hull2);

  auto i_height = std::max(
      (float)0,
      std::min(
          ((b1.center.z() + b1.height / 2) - (b2.center.z() - b2.height / 2)),
          ((b2.center.z() + b2.height / 2) - (b1.center.z() - b1.height / 2))));

  auto u_height = std::max(
      ((b1.center.z() + b1.height / 2) - (b2.center.z() - b2.height / 2)),
      ((b2.center.z() + b2.height / 2) - (b1.center.z() - b1.height / 2)));

  i = i * i_height;                                   // 交体积
  auto u{area1 * b1.height + area2 * b2.height - i};  // union体积

  if (u == 0) {
    throw std::out_of_range("Invalid iou, u is 0");
  }

  return i / u;  // IOU
}

float Cost::giou2d(const Box& b1, const Box& b2) {
  polygon_t poly1;
  polygon_t poly2;
  polygon_t poly3;

  for (auto pp : b1.polygon) {
    point_t p1{pp.x(), pp.y()};
    bg::append(poly1, p1);
    bg::append(poly3, p1);
  }

  for (auto pp : b2.polygon) {
    point_t p2{pp.x(), pp.y()};
    bg::append(poly2, p2);
    bg::append(poly3, p2);
  }

  polygon_t hull1;
  polygon_t hull2;
  bg::convex_hull(poly1, hull1);
  bg::convex_hull(poly2, hull2);

  std::deque<polygon_t> output;
  auto isOK = bg::intersection(hull1, hull2, output);
  if (!isOK) {
    // throw std::logic_error("polygen intersection error!");
  }

  double i{0};
  BOOST_FOREACH (polygon_t const& p, output) {
    i += bg::area(p);
  }  // 求交面积

  auto area1 = bg::area(hull1);
  auto area2 = bg::area(hull2);
  auto u{area1 + area2 - i};  // union面积

  // 求凸包
  polygon_t hull3;
  bg::convex_hull(poly3, hull3);

  auto c{bg::area(hull3)};  // 凸包面积

  if (u == 0 || c == 0) {
    // throw std::out_of_range("Invalid giou, u is 0 or c is 0");
  }

  return i / u - (c - u) / c;  // GIOU
}

float Cost::giou3d(const Box& b1, const Box& b2) {
  polygon_t poly1;
  polygon_t poly2;
  polygon_t poly3;

  for (auto pp : b1.polygon) {
    point_t p1{pp.x(), pp.y()};
    bg::append(poly1, p1);
    bg::append(poly3, p1);
  }

  for (auto pp : b2.polygon) {
    point_t p2{pp.x(), pp.y()};
    bg::append(poly2, p2);
    bg::append(poly3, p2);
  }

  polygon_t hull1;
  polygon_t hull2;
  bg::convex_hull(poly1, hull1);
  bg::convex_hull(poly2, hull2);

  std::deque<polygon_t> output;
  auto isOK = bg::intersection(hull1, hull2, output);
  if (!isOK) {
    throw std::logic_error("polygen intersection error!");
  }

  double i{0};
  BOOST_FOREACH (polygon_t const& p, output) {
    i += bg::area(p);
  }  // 求交面积

  auto area1 = bg::area(hull1);
  auto area2 = bg::area(hull2);

  // 求凸包
  polygon_t hull3;
  bg::convex_hull(poly3, hull3);

  auto c{bg::area(hull3)};  // 凸包面积

  auto i_height = std::max(
      (float)0,
      std::min(
          ((b1.center.z() + b1.height / 2) - (b2.center.z() - b2.height / 2)),
          ((b2.center.z() + b2.height / 2) - (b1.center.z() - b1.height / 2))));

  auto u_height = std::max(
      ((b1.center.z() + b1.height / 2) - (b2.center.z() - b2.height / 2)),
      ((b2.center.z() + b2.height / 2) - (b1.center.z() - b1.height / 2)));

  i = i * i_height;
  c = c * u_height;
  auto u{area1 * b1.height + area2 * b2.height - i};  // union体积

  if (u == 0 || c == 0) {
    throw std::out_of_range("Invalid giou, u is 0 or c is 0");
  }

  return i / u - (c - u) / c;  // GIOU
}

void Cost::object_to_box(ObjectTrackPtr const& track, Box& b) noexcept {
  b.center = track->current_object_->center;
  // b.width = track->current_object_->size[1];
  // b.length = track->current_object_->size[0];
  // b.height = track->current_object_->size[2];
  b.width = track->current_object_->width;
  b.length = track->current_object_->length;
  b.height = track->current_object_->height;
  for (size_t i = 0; i < 4; i++) {
    b.polygon[i].x() = track->current_object_->polygon.at(i).x();
    b.polygon[i].y() = track->current_object_->polygon.at(i).y();
    b.polygon[i].z() = 0.0f;
  }
}

void Cost::object_to_box(TrackedObjectPtr const& track, Box& b) noexcept {
  b.center.x() = track->center.x();
  b.center.y() = track->center.y();
  b.center.z() = track->center.z();
  // b.width = track->size[1];
  // b.length = track->size[0];
  // b.height = track->size[2];
  b.width = track->width;
  b.length = track->length;
  b.height = track->height;
  for (size_t i = 0; i < 4; i++) {
    b.polygon[i].x() = track->polygon.at(i).x();
    b.polygon[i].y() = track->polygon.at(i).y();
    b.polygon[i].z() = 0.0f;
  }
}

void Cost::object_to_box(ObjectTrackPtr const& track, Box& b,
                         const Eigen::Matrix4f& pos) noexcept {
  Eigen::Vector4f temp_center =
      pos * Eigen::Vector4f{track->current_object_->center.x(),
                            track->current_object_->center.y(),
                            track->current_object_->center.z(), 1.0f};

  b.center.x() = temp_center.x();
  b.center.y() = temp_center.y();
  b.center.z() = temp_center.z();
  // b.width = track->current_object_->size[1];
  // b.length = track->current_object_->size[0];
  // b.height = track->current_object_->size[2];
  b.width = track->current_object_->width;
  b.length = track->current_object_->length;
  b.height = track->current_object_->height;
  for (size_t i = 0; i < 4; i++) {
    Eigen::Vector4f temp_polygen =
        pos * Eigen::Vector4f{track->current_object_->polygon.at(i).x(),
                              track->current_object_->polygon.at(i).y(), 0.0f,
                              1.0f};
    b.polygon[i].x() = temp_polygen.x();
    b.polygon[i].y() = temp_polygen.y();
    b.polygon[i].z() = 0.0f;
  }
}
void Cost::object_to_box(TrackedObjectPtr const& track, Box& b,
                         const Eigen::Matrix4f& pos) noexcept {
  Eigen::Vector4f temp_center =
      pos * Eigen::Vector4f{track->center.x(), track->center.y(),
                            track->center.z(), 1.0f};

  b.center.x() = temp_center.x();
  b.center.y() = temp_center.y();
  b.center.z() = temp_center.z();

  // b.width = track->size[1];
  // b.length = track->size[0];
  // b.height = track->size[2];
  b.width = track->width;
  b.length = track->length;
  b.height = track->height;
  for (size_t i = 0; i < 4; i++) {
    Eigen::Vector4f temp_polygen =
        pos * Eigen::Vector4f{track->polygon.at(i).x(),
                              track->polygon.at(i).y(), 0.0f, 1.0f};
    b.polygon[i].x() = temp_polygen.x();
    b.polygon[i].y() = temp_polygen.y();
    b.polygon[i].z() = 0.0f;
  }
}

}  // namespace perception
}  // namespace autodrive
